// FreeRTOS include files
#include "..\..\FreeRTOS\source\include\FreeRTOS.h"
#include "..\..\FreeRTOS\source\include\task.h"
#include "..\..\FreeRTOS\source\include\croutine.h"
#include "..\..\FreeRTOS\source\include\semphr.h"

#ifdef CHIP_GPCE063
#include "..\..\BSP\include\GPCE063.h"
#endif

#ifdef CHIP_GPCE206X
#include "..\..\BSP\include\GPCE206x.h"
#endif

#include "SACM.h"

// I2C Driver
#include "..\..\BSP\drivers\Drv_i2c.h"

//**************************************************************************
// Contant Defintion Area
//**************************************************************************
#define MaxSpeechNum        7       // Max. of speech in resource
#define MaxVolumeNum        16      // Max. of volume settings

#define Foreground          0
#define Background          1
//#define ServiceType       Foreground
#define ServiceType         Background

// Breathing LED demo
const unsigned char curve[] = {
    1,     1,     1,     1,     1,     1,     1,     1,
    1,     1,     1,     1,     1,     1,     1,     1,
    1,     1,     1,     2,     2,     2,     2,     2,
    2,     2,     3,     3,     3,     3,     3,     3,
    4,     4,     4,     4,     4,     5,     5,     5,
    5,     6,     6,     6,     6,     7,     7,     7,
    8,     8,     8,     8,     9,     9,     9,    10,
   10,    10,    11,    11,    11,    12,    12,    13,
   13,    13,    14,    14,    15,    15,    15,    16,
   16,    17,    17,    18,    18,    18,    19,    19,
   20,    20,    21,    21,    22,    22,    23,    23,
   24,    24,    25,    25,    26,    26,    27,    27,
   28,    29,    29,    30,    30,    31,    31,    32,
   33,    33,    34,    34,    35,    36,    36,    37,
   38,    38,    39,    39,    40,    41,    41,    42,
   43,    43,    44,    45,    46,    46,    47,    48,
   48,    49,    50,    50,    51,    52,    53,    53,
   54,    55,    56,    56,    57,    58,    59,    59,
   60,    61,    62,    62,    63,    64,    65,    66,
   66,    67,    68,    69,    70,    70,    71,    72,
   73,    74,    75,    75,    76,    77,    78,    79,
   80,    80,    81,    82,    83,    84,    85,    86,
   87,    87,    88,    89,    90,    91,    92,    93,
   94,    95,    95,    96,    97,    98,    99,   100,
  101,   102,   103,   104,   105,   106,   106,   107,
  108,   109,   110,   111,   112,   113,   114,   115,
  116,   117,   118,   119,   120,   121,   122,   122,
  123,   124,   125,   126,   127,   128,   129,   130,
  131,   132,   133,   134,   135,   136,   137,   138,
  139,   140,   141,   142,   143,   144,   145,   146,
  147,   148,   149,   150,   151,   152,   153,   154,
  155,   156
};

//**************************************************************************
// Function Call Publication Area
//**************************************************************************
//void SPI_Initial();
void USER_A1600_SetStartAddr(int SpeechIndex);
void USER_A1600_SetStartAddr_Con(int SpeechIndex);
float FastInvSqrt(float x);
void Reset_Watchdog(void);
int BSP_INIT(void);
void loop_test_01(void *pvParameters); // Play audio
void loop_test_02(void *pvParameters); // I2C
void loop_test_03(void *pvParameters); // LED
void CDecoder(void *pvParameters);

portTickType xTick = 0;
portSTACK_TYPE stack[configTOTAL_HEAP_SIZE];



int main()
{
    unsigned int SpeechIndex = 0;

    #if 0 // checking size
    // checking size of portSTACK_TYPE
    count = sizeof(portSTACK_TYPE)*configTOTAL_HEAP_SIZE;
    // checking size of char, unsigned char
    count = sizeof(char);           // 1 word
    count = sizeof(unsigned char);  // 1 word
    // checking size of int, unsigned int
    count = sizeof(int);            // 1 word
    count = sizeof(unsigned int);   // 1 word
    // checking size of short, unsigned short
    count = sizeof(short);          // 1 word
    count = sizeof(unsigned short); // 1 word
    // checking size of long, unsigned long
    count = sizeof(long);           // 2 words
    count = sizeof(unsigned long);  // 2 words
    // checking size of float
    count = sizeof(float);          // 2 words
    // checking size of double
    count = sizeof(double);         // 2 words
    #endif

    BSP_INIT();

    /* Create the tasks defined within this file. */
    //xTaskCreate(CDecoder, (signed portCHAR *)"CDecoder", configMINIMAL_STACK_SIZE, NULL, 4, NULL );
    xTaskCreate(loop_test_01, (signed portCHAR *)"loop_test_01", configMINIMAL_STACK_SIZE, (void*)&SpeechIndex, 5, NULL );
    xTaskCreate(loop_test_02, (signed portCHAR *)"loop_test_02", configMINIMAL_STACK_SIZE, (void*)&SpeechIndex, 3, NULL );

    /* In this port, to use preemptive scheduler define configUSE_PREEMPTION
    as 1 in portmacro.h.  To use the cooperative scheduler define
    configUSE_PREEMPTION as 0. */
    vTaskStartScheduler();

    // RunSchedular fail!!
    while(1)
    {
        Reset_Watchdog();
    }

    return 0;
}

//#define LOOP_PLAY
#define GKEY_PLAY
void loop_test_01(void *pvParameters)
{
    unsigned int* SpeechIndex = pvParameters;
    unsigned int delay = 100;
    unsigned int old_gkey = 0, new_gkey = 0, count_1 = 0,  count_2 = 0;

    SACM_A1600_Initial(); // A1600 initial

    vTaskDelay( 1000 / portTICK_RATE_MS );
    SACM_A1600_Play(0, DAC1 + DAC2, Ramp_Up + Ramp_Dn);  // Play welcome to tom's wheel
    vTaskDelay( 6000 / portTICK_RATE_MS );

    while(1)
    {
        #ifdef LOOP_PLAY
        // Start
        // SpeechIndex = 0, Say...  welcome to tom's wheel
        // SpeechIndex = 1, Say...  1
        // SpeechIndex = 2, Say...  2
        // SpeechIndex = 3, Say...  3
        // SpeechIndex = 4, Say...  4
        // SpeechIndex = 5, Say...  5
        // SpeechIndex = 6, Say...  6
        // Go back to start

        vTaskDelay( delay / portTICK_RATE_MS );
        SACM_A1600_Play(*SpeechIndex, DAC1 + DAC2, Ramp_Up + Ramp_Dn);

        if(++(*SpeechIndex) >= MaxSpeechNum)        // Play next speech
            *SpeechIndex = 0;
        #endif

        #ifdef GKEY_PLAY

        new_gkey = *SpeechIndex; // Get G Key.

        if(old_gkey != new_gkey) // Is G key changed ?
            count_1++;
        else
            count_1 = 0;

        if(count_1 > 15) // Play side if G key have changed and hold to 1.5 second
        {
            old_gkey = new_gkey; // Save old G key.
            count_1 = 0;
            vTaskDelay( delay / portTICK_RATE_MS );
            SACM_A1600_Play(*SpeechIndex, DAC1 + DAC2, Ramp_Up + Ramp_Dn); // Play side speech
        }
        else
        {
            vTaskDelay( delay / portTICK_RATE_MS );
            
            if(count_2 > 600) // Play welcome to tom's wheel every 1 minute
            {
                vTaskDelay( 1000 / portTICK_RATE_MS );
                SACM_A1600_Play(0, DAC1 + DAC2, Ramp_Up + Ramp_Dn);
                vTaskDelay( 6000 / portTICK_RATE_MS );
                count_2 = 0;
            }
            count_2++;
        }
        #endif

        Reset_Watchdog();

    }
}

#define ACC_MAX  ((0.8f/0.25f)*1000*4)
#define ACC_MIN  -ACC_MAX

void loop_test_02(void *pvParameters)
{
    unsigned int* SpeechIndex = pvParameters;
    unsigned int delay = 90;
    uint8_t buf[2] = {0, 0};
    int ax = 0, ay = 0, az = 0;
    double norm = 0,nax = 0, nay = 0, naz = 0;

    asm("FIQ on");

    i2cInit();

    while(1)
    {
        vTaskDelay( delay / portTICK_RATE_MS );

        // To read back chip id from BMA180 ( 7bits address = 0x41 )
        i2cRead(0x41, 0x0, 1, (uint8_t*)&buf); // chip id - 0x03

        // Read X
        ax =0; buf[0] = 0; buf[1] = 0;
        i2cRead(0x41, 0x2, 2, (uint8_t*)&buf);
        ax = ((buf[1]<<8) | buf[0]) & 0xFFFC;
        Reset_Watchdog();

        // Read Y
        ay =0; buf[0] = 0; buf[1] = 0;
        i2cRead(0x41, 0x4, 2, (uint8_t*)&buf);
        ay = ((buf[1]<<8) | buf[0]) & 0xFFFC;
        Reset_Watchdog();

        // Read Z
        az =0; buf[0] = 0; buf[1] = 0;
        i2cRead(0x41, 0x6, 2, (uint8_t*)&buf);
        az = ((buf[1]<<8) | buf[0]) & 0xFFFC;
        Reset_Watchdog();

        norm = ax*ax + ay*ay +az*az;
        norm = FastInvSqrt(norm);

        nax = ax * norm;
        nay = ay * norm;
        naz = az * norm;

        if(ax > ACC_MAX)      // side 1
            *SpeechIndex = 1;
        else if(ax < ACC_MIN) // side 2
            *SpeechIndex = 2;
        else if(ay > ACC_MAX) // side 3
            *SpeechIndex = 3;
        else if(ay < ACC_MIN) // side 4
            *SpeechIndex = 4;
        else if(az > ACC_MAX) // side 5
            *SpeechIndex = 5;
        else if(az < ACC_MIN) // side 6
            *SpeechIndex = 6;
    }
}

void loop_test_03(void *pvParameters)
{
    const unsigned char* curve = pvParameters;
    int pos = 0;   // Index to PWM's duty cycle table (= brightness)
    int new_ccr1 = 1;

    asm("FIQ on");

    while(1)
    {
        if (pos < 500) {
            new_ccr1 = curve[pos++ >> 1];
        } else if (pos < 1000) {
            new_ccr1 = curve[(999 - pos++) >> 1];
        } else {
            pos = 0;
        }
    }
}


void CDecoder(void *pvParameters)
{
    unsigned Key = 0;
    unsigned SpeechIndex = 0;
    unsigned VolumeIndex = 9;
    unsigned DAC_FIR_Type = C_DAC_FIR_Type2;
    unsigned PlayCon = 0;

    SACM_A1600_Initial();       // A1600 initial

    while(1)
    {
        Key = SP_GetCh();
        switch(Key)
        {
            case 0x0000:
                break;

            case 0x0001:    // IOA0 + Vcc
                PlayCon = 0;
                SACM_A1600_Play(SpeechIndex, DAC1 + DAC2, Ramp_Up + Ramp_Dn);       // play speech
                break;

            case 0x0002:    // IOA1 + Vcc
                PlayCon = 0;
                SACM_A1600_Stop();                      // stop
                break;

            case 0x0004:    // IOA2 + Vcc
                SACM_A1600_Pause();                     // playback pause
                break;

            case 0x0008:    // IOA3 + Vcc
                SACM_A1600_Resume();                    // playback resuem
                break;

            case 0x0010:    // IOA4 + Vcc
                if(++SpeechIndex >= MaxSpeechNum)       // next speech
                    SpeechIndex = 0;
                PlayCon = 0;
                SACM_A1600_Play(SpeechIndex, DAC1 + DAC2, Ramp_Up + Ramp_Dn);       // play next speech
                break;

            case 0x0020:    // IOA5 + Vcc
                if(++VolumeIndex >= MaxVolumeNum)
                    VolumeIndex = 0;
                USER_A1600_Volume(VolumeIndex);         // volume up
                break;

            case 0x0040:    // IOA6 + Vcc
                if(++DAC_FIR_Type > C_DAC_FIR_Type3)
                    DAC_FIR_Type = C_DAC_FIR_Type0;
                SACM_A1600_DA_FIRType(DAC_FIR_Type);    // change DAC filter type
                break;

            case 0x0080:    // IOA7 + Vcc
                PlayCon = 1;
                SpeechIndex = 0;
                USER_A1600_SetStartAddr(SpeechIndex);
                SACM_A1600_Play(Manual_Mode_Index, DAC1 + DAC2, Ramp_Up);
                break;

            default:
                break;
        } // end of switch

        if(PlayCon)
        {
            if(SACM_A1600_Check_Con() == -1)
            {
                if(++SpeechIndex >= MaxSpeechNum)       // next speech
                    SpeechIndex = 0;
                USER_A1600_SetStartAddr_Con(SpeechIndex);
                SACM_A1600_Play_Con(Manual_Mode_Index, DAC1 + DAC2, Ramp_Dn);
            }
        }

        if(ServiceType == Foreground)
            SACM_A1600_ServiceLoop();

        System_ServiceLoop();
    } // end of while
}

void Reset_Watchdog(void)
{
    P_Watchdog_Clear = C_Watchdog_Clear;
}

int BSP_INIT(void)
{
    init_heap((size_t)stack,configTOTAL_HEAP_SIZE);

    #if 0
    // Config System Clock
    P_Int_Ctrl = 0x0000;
    P_Int_Status = 0xFFFF;
    P_System_Clock = 0x0098;

    // Config IOB as Output Port
    P_IOB_Data->data   = 0x0000;
    P_IOB_Attrib->data = 0xFFFF;
    P_IOB_Dir->data    = 0xFFFF;
    #endif

    System_Initial();           // System initial

    return 0;
}

/*
    Fast inverse square root :
    http://blog.quenta.org/2012/09/0x5f3759df.html
    http://en.wikipedia.org/wiki/Fast_inverse_square_root
*/
float FastInvSqrt(float x) {
  float xhalf = 0.5f * x;
  int i = *(int*)&x;         // evil floating point bit level hacking
  i = 0x5f3759df - (i >> 1);  // what the fuck?
  x = *(float*)&i;
  x = x*(1.5f-(xhalf*x*x));
  return x;
}

